#include "stdafx.h"
#include "PhysicsSystem.h"

#include "CollisionData.h"
#include "CollisionDetection.h"
#include "CollisionResolution.h"

using namespace mphys;

PhysicsSystem::PhysicsSystem()
	: mBoxes(200), mSpheres(1200), mPlanes(20), mNextID(INCORRECT_RB_ID + 1)
{
	mCollData = new CollisionData;
}

PhysicsSystem::~PhysicsSystem()
{
	delete mCollData;
}

RigidBody & PhysicsSystem::createBoxRigidBody(RigidBody::ID id)
{
	Box box;
	box.System = this;
	mBoxes.add(box);
	addToRigidBodyMap(mBoxes.back(), id);
	return mBoxes.back();
}

RigidBody & PhysicsSystem::createSphereRigidBody(RigidBody::ID id)
{
	Sphere sphere;
	sphere.System = this;
	mSpheres.add(sphere);
	addToRigidBodyMap(mSpheres.back(), id);
	return mSpheres.back();
}

RigidBody & PhysicsSystem::createPlaneRigidBody(RigidBody::ID id)
{
	Plane plane;
	plane.System = this;
	mPlanes.add(plane);
	addToRigidBodyMap(mPlanes.back(), id);
	return mPlanes.back();
}

RigidBody & PhysicsSystem::createBoxCopy( const Box & src )
{
	mBoxes.add(src);
	addToRigidBodyMap(mBoxes.back(), INCORRECT_RB_ID);
	return mBoxes.back();
}

RigidBody & PhysicsSystem::createSphereCopy( const Sphere & src )
{
	mSpheres.add(src);
	addToRigidBodyMap(mSpheres.back(), INCORRECT_RB_ID);
	return mSpheres.back();
}

RigidBody & PhysicsSystem::createPlaneCopy( const Plane & src )
{
	mPlanes.add(src);
	addToRigidBodyMap(mPlanes.back(), INCORRECT_RB_ID);
	return mPlanes.back();
}

RigidBody & PhysicsSystem::findRigidBody(RigidBody::ID id)
{
	assert(mRigidBodies.find(id) != mRigidBodies.end());
	return *mRigidBodies[id];
}

void PhysicsSystem::removeRigidBody(RigidBody & rb)
{
	Box * box = dynamic_cast<Box*>(&rb);
	if(box != NULL)
	{
		// if obj to remove not the last object in the list, swap them and update the moved object from las position
		// so it can be found by its users
		Box & lastObj = mBoxes.back();
		if(box != &lastObj)
		{
			std::swap(*box, lastObj);
			updateRigidBodyMapEntry(*box);
		}
		// remove last object's entry from objects map
		removeRigidBodyMapEntry(lastObj);
		mBoxes.removeLast();
		return;
	}

	Sphere * sphere = dynamic_cast<Sphere*>(&rb);
	if(sphere != NULL)
	{
		Sphere & lastObj = mSpheres.back();
		if(sphere != &lastObj)
		{
			std::swap(*sphere, lastObj);
			updateRigidBodyMapEntry(*sphere);
		}
		removeRigidBodyMapEntry(lastObj);
		mSpheres.removeLast();
		return;
	}

	Plane * plane = dynamic_cast<Plane*>(&rb);
	if(plane != NULL)
	{
		Plane & lastObj = mPlanes.back();
		if(plane != &lastObj)
		{
			std::swap(*plane, lastObj);
			updateRigidBodyMapEntry(*plane);
		}
		removeRigidBodyMapEntry(lastObj);
		mPlanes.removeLast();
		return;
	}
}

void PhysicsSystem::update( float dt )
{
	uint spheresSize = mSpheres.NumElements;
	uint planesSize = mPlanes.NumElements;
	uint boxesSize = mBoxes.NumElements;


	// integrate
	
	for (uint i = 0; i < boxesSize ; i++)
	{
		mBoxes[i].update(dt);
		mBoxes[i].updateBoxData();
	}

	
	for (uint i = 0; i < spheresSize ; i++)
	{
		mSpheres[i].update(dt);
	}

	mCollData->reset();

	for (uint i = 0; i < spheresSize ; i++)
	{
		for (uint j = i + 1; j < spheresSize ; j++)
		{
			sphereSphereCollision(mSpheres[i], mSpheres[j], *mCollData);
		}
	}

	for (uint i = 0; i < planesSize ; i++)
	{
		for (uint j = 0; j < spheresSize ; j++)
		{
			planeSphereCollision(mPlanes[i], mSpheres[j], *mCollData);
		}
	}

	for (uint i = 0; i < boxesSize ; i++)
	{
		for (uint j = 0; j < spheresSize ; j++)
		{
			boxSphereCollision(mBoxes[i], mSpheres[j], *mCollData);
		}
	}

	for (uint i = 0; i < planesSize ; i++)
	{
		for (uint j = 0; j < boxesSize ; j++)
		{
			if(mBoxes[j].MassInverse > 0.0f)
				boxPlaneCollision(mBoxes[j], mPlanes[i], *mCollData);
		}
	}

	

	for (uint i = 0; i < boxesSize ; i++)
	{
		for (uint j = i + 1; j < boxesSize ; j++)
		{
			bool lol = boxBoxCollision(mBoxes[i], mBoxes[j], *mCollData);
		}
	}

	// collision resolution
	resolveContactsComplete(*mCollData, dt);



	

	//Sleep(50);
}

void PhysicsSystem::addToRigidBodyMap(RigidBody & rb, RigidBody::ID id)
{
	if(id == INCORRECT_RB_ID)
		id = mNextID++;
	
	rb.Id = id;
	mRigidBodies[id] = &rb;
}

void PhysicsSystem::updateRigidBodyMapEntry(RigidBody & rb)
{
	mRigidBodies[rb.Id] = &rb;
}

void PhysicsSystem::removeRigidBodyMapEntry(RigidBody & rb)
{
	mRigidBodies.erase(rb.Id);
	rb.Id = INCORRECT_RB_ID;
}